/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

//import com.sun.squawk.util.MathUtils;
//import edu.wpi.first.wpilibj.CounterBase.EncodingType;
//import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
//import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import file.DataRecorder;

/**
 *
 * @author Carl Burger and Conner Davis - Team 708
 */
public class Drivetrain{

    /**
     * paraphased from LabVIEW Help for encoder setup:
     * setDistancePerPulse scales the pulses of the encoder into engineering units
     * for the return value of the getDistance method. For example, if the wheel
     * diameter is six inches and the encoder pulses 64 times per rotation of
     * the wheel, then a scaling factor of 6*pi / 64 = .2945 returns the number of
     * inches the wheel has turned. In this case, you input .2945.
     * Last year we specified counts per foot and inverted it.
     * 234.4 and 220.8 were the values which yielded .004266 and .004528.
     */

    /*
        Measured 10 feet, drove robot and recorded the encoder counts
        Divide counts by 10 to get counts/ft
        Invert to get feet/count
        Multiply by 12 to get inches/count
        Using last year's value of 234.4, the distance in inches was off by ~20"
           count dist(in) but should have been 120
        FL 2759 141.28   275.9
        FR 2740 140.31   274
        RL 2632 134.76   263.2
        RR 2754 141.02   275.4

        headings to use in the autonomous script:
         0 toward side (away from center line)
        90 toward ball
       -90 away from ball
     */

    // compute inputs for setDistancePerPulse method calls further down
    private final double kFLCountsPerFoot  = 275.9;
    private final double kFRCountsPerFoot  = 274;
    private final double kRLCountsPerFoot  = 263.2;
    private final double kRRCountsPerFoot  = 275.4;

    private final double kFLFeetPerCount   = 1/kFLCountsPerFoot;
    private final double kFRFeetPerCount   = 1/kFRCountsPerFoot;
    private final double kRLFeetPerCount   = 1/kRLCountsPerFoot;
    private final double kRRFeetPerCount   = 1/kRRCountsPerFoot;

    private final double kFLInchesPerCount = kFLFeetPerCount * 12;
    private final double kFRInchesPerCount = kFRFeetPerCount * 12;
    private final double kRLInchesPerCount = kRLFeetPerCount * 12;
    private final double kRRInchesPerCount = kRRFeetPerCount * 12;
    
    RobotDrive   fourMotorDrive;

    Gyroscope    gyroscope;
    
//    DigitalInput frontLeftDriveAchannel;  // aSource for left front encoder
//    DigitalInput frontLeftDriveBchannel;  // bSource for left front encoder
//    DigitalInput frontRightDriveAchannel; // aSource for right front encoder
//    DigitalInput frontRightDriveBchannel; // bSource for right front encoder
//    DigitalInput rearLeftDriveAchannel;   // aSource for left rear encoder
//    DigitalInput rearLeftDriveBchannel;   // bSource for left rear encoder
//    DigitalInput rearRightDriveAchannel;  // aSource for right rear encoder
//    DigitalInput rearRightDriveBchannel;  // bSource for right rear encoder
//
//    Encoder      frontLeftDriveEncoder;
//    Encoder      frontRightDriveEncoder;
//    Encoder      rearLeftDriveEncoder;
//    Encoder      rearRightDriveEncoder;


    // maintain previous values so that the driver station is only updated if
    // the displayed information changes
    int previousFrontLeftCount  = 0;    // encoder counts
    int previousFrontRightCount = 0;
    int previousRearLeftCount   = 0;
    int previousRearRightCount  = 0;

    double previousMagnitude = 0;       // autonomous inputs
    double previousDirection = 0;
    double previousRotation  = 0;

    double previousHeading = 0;         // gyro

    MessageCenter messageCenter;

    DataRecorder  dataRecorder;
    

    private static Drivetrain m_instance = null;



    /**
     *  Get an instance of the Drivetrain
     */
    public static Drivetrain getInstance() {
        if (m_instance == null) {
            m_instance = new Drivetrain();

        }
        return m_instance;
    }

    private Drivetrain() {

        // Sensors

        gyroscope = Gyroscope.getInstance();

//        frontLeftDriveAchannel  = new DigitalInput(LogomotionRobot.kFrontLeftDriveAchannel);
//        frontLeftDriveBchannel  = new DigitalInput(LogomotionRobot.kFrontLeftDriveBchannel);
//        frontRightDriveAchannel = new DigitalInput(LogomotionRobot.kFrontRightDriveAchannel);
//        frontRightDriveBchannel = new DigitalInput(LogomotionRobot.kFrontRightDriveBchannel);
//        rearLeftDriveAchannel   = new DigitalInput(LogomotionRobot.kRearLeftDriveAchannel);
//        rearLeftDriveBchannel   = new DigitalInput(LogomotionRobot.kRearLeftDriveBchannel);
//        rearRightDriveAchannel  = new DigitalInput(LogomotionRobot.kRearRightDriveAchannel);
//        rearRightDriveBchannel  = new DigitalInput(LogomotionRobot.kRearRightDriveBchannel);


        fourMotorDrive   = new RobotDrive(LogomotionRobot.kFrontLeftController,
                                          LogomotionRobot.kRearLeftController,
                                          LogomotionRobot.kFrontRightController,
                                          LogomotionRobot.kRearRightController);
        fourMotorDrive.setInvertedMotor(RobotDrive.MotorType.kFrontLeft, true); //t   //t
        fourMotorDrive.setInvertedMotor(RobotDrive.MotorType.kFrontRight, false);  //f //t
        fourMotorDrive.setInvertedMotor(RobotDrive.MotorType.kRearLeft, true);   //t //f
        fourMotorDrive.setInvertedMotor(RobotDrive.MotorType.kRearRight, false); //f  //f

        fourMotorDrive.setExpiration(5.0);
        fourMotorDrive.setSafetyEnabled(true);

            // optical encoders

            // left - invert direction false   decoding type 4x   distance per count 1
            // num samples to avg 1  min rate 20
//        frontLeftDriveEncoder   = new Encoder(frontLeftDriveAchannel, frontLeftDriveBchannel,
//                                              false, EncodingType.k4X);
//        frontRightDriveEncoder  = new Encoder(frontRightDriveAchannel, frontRightDriveBchannel,
//                                              true, EncodingType.k4X);
//        rearLeftDriveEncoder    = new Encoder(rearLeftDriveAchannel, rearLeftDriveBchannel,
//                                              false, EncodingType.k4X);
//        rearRightDriveEncoder   = new Encoder(rearRightDriveAchannel, rearRightDriveBchannel,
//                                              true, EncodingType.k4X);
//
//        frontLeftDriveEncoder.setDistancePerPulse(kFLInchesPerCount);
//        frontRightDriveEncoder.setDistancePerPulse(kFRInchesPerCount);
//        rearLeftDriveEncoder.setDistancePerPulse(kRLInchesPerCount);
//        rearRightDriveEncoder.setDistancePerPulse(kRRInchesPerCount);

        /**
         * from LabVIEW Help: Minimum Rate specifies the minimum rate, in pulses
         * per second, at which the CompactRIO device must receive pulses from
         * the encoder before the device considers the encoder stopped. The
         * default is 20. Use the Get VI to return the stopped status of the encoder.
         */
//        frontLeftDriveEncoder.setMinRate(20);
//        frontRightDriveEncoder.setMinRate(20);
//        rearLeftDriveEncoder.setMinRate(20);
//        rearRightDriveEncoder.setMinRate(20);
//
//        frontLeftDriveEncoder.start();
//        frontRightDriveEncoder.start();
//        rearLeftDriveEncoder.start();
//        rearRightDriveEncoder.start();

        resetEncoders();

//        int frontLeftCount  = frontLeftDriveEncoder.get();
//        int frontRightCount = frontRightDriveEncoder.get();
//        int rearLeftCount   = rearLeftDriveEncoder.get();
//        int rearRightCount  = rearRightDriveEncoder.get();

        messageCenter = MessageCenter.getInstance();
        messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine1,
                              "Driver Joystick");
        messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine1,
                              "Encdrs Count Dist Dir");

//        messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine2,
//                              "FL " + frontLeftCount + " " +
//                              MathUtils.round(frontLeftDriveEncoder.getDistance()) + " " +
//                              frontLeftDriveEncoder.getDirection());
//        messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine3,
//                              "FR " + frontRightCount + " " +
//                              MathUtils.round(frontRightDriveEncoder.getDistance()) + " " +
//                              frontRightDriveEncoder.getDirection());
//        messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine4,
//                              "RL " + rearLeftCount + " " +
//                              MathUtils.round(rearLeftDriveEncoder.getDistance()) + " " +
//                              rearLeftDriveEncoder.getDirection());
//        messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine5,
//                              "RR " + rearRightCount + " " +
//                              MathUtils.round(rearRightDriveEncoder.getDistance()) + " " +
//                              rearRightDriveEncoder.getDirection());

        dataRecorder = DataRecorder.getInstance();
    }

    public void resetEncoders() {

//        frontLeftDriveEncoder.reset();
//        frontRightDriveEncoder.reset();
//        rearLeftDriveEncoder.reset();
//        rearRightDriveEncoder.reset();
    }

    public void outputStatus(){
        displayGyro();
        displayEncoders();

    }

    /*
     * Records information to the datarecorder.
     * Note: not everything in the drivetrain is recorded here.
     */
    public void recordStatus(){
        dataRecorder.record("gyro",gyroscope.getAngle());

    }

    private void displayEncoders() {

//        int frontLeftCount  = frontLeftDriveEncoder.get();
//        int frontRightCount = frontRightDriveEncoder.get();
//        int rearLeftCount   = rearLeftDriveEncoder.get();
//        int rearRightCount  = rearRightDriveEncoder.get();
//
//        long frontLeftDist  = MathUtils.round(frontLeftDriveEncoder.getDistance());
//        long frontRightDist = MathUtils.round(frontRightDriveEncoder.getDistance());
//        long rearLeftDist   = MathUtils.round(rearLeftDriveEncoder.getDistance());
//        long rearRightDist  = MathUtils.round(rearRightDriveEncoder.getDistance());
//
//        if (previousFrontLeftCount  != frontLeftCount  ||
//            previousFrontRightCount != frontRightCount ||
//            previousRearLeftCount   != rearLeftCount   ||
//            previousRearRightCount  != rearRightCount) {
//
////            System.out.println("Encoder FL " + frontLeftCount +
////                               "  FR " + frontRightCount +
////                               "  RL " + rearLeftCount +
////                               "  RR " + rearRightCount);
//
//            messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine2,
//                                  "FL " + frontLeftCount + " " + frontLeftDist + " " +
//                                  frontLeftDriveEncoder.getDirection());
//            messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine3,
//                                  "FR " + frontRightCount + " " + frontRightDist + " " +
//                                  frontRightDriveEncoder.getDirection());
//            messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine4,
//                                  "RL " + rearLeftCount + " " + rearLeftDist + " " +
//                                  rearLeftDriveEncoder.getDirection());
//            messageCenter.println(MessageCenter.kEncodersScreen, MessageCenter.kLine5,
//                                  "RR " + rearRightCount + " " + rearRightDist + " " +
//                                  rearRightDriveEncoder.getDirection());
//
//            previousFrontLeftCount  = frontLeftCount;
//            previousFrontRightCount = frontRightCount;
//            previousRearLeftCount   = rearLeftCount;
//            previousRearRightCount  = rearRightCount;
//
//            dataRecorder.record("frontLeftDist",frontLeftDist);
//            dataRecorder.record("frontRightDist",frontRightDist);
//            dataRecorder.record("rearLeftDist",rearLeftDist);
//            dataRecorder.record("rearRightDist",rearRightDist);
//
//        }
    }

    private void displayCommandedInputs(double magnitude, double direction, double rotation) {

        if ( (previousMagnitude != magnitude) ||
             (previousDirection != direction) ||
             (previousRotation  != rotation) ) {

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine2,
                                  "X ignored Y ignored");

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine3,
                                  "Z " + messageCenter.formatDouble(rotation, 6));

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine4,
                                  "M " + messageCenter.formatDouble(magnitude, 6) +
                                  " D " + messageCenter.formatDouble(direction, 6));

            previousMagnitude = magnitude;
            previousDirection = direction;
            previousRotation  = rotation;
        }
    }

    private void displayGyro() {

        double currHeading = gyroscope.getAngle();

        if ( previousHeading != currHeading) {

            messageCenter.println(MessageCenter.kDriverJoystickScreen, MessageCenter.kLine5,
                                  "Gyro " + messageCenter.formatDouble(currHeading, 6));

            previousHeading = currHeading;
        }
    }

    public void drive(double magnitude, double direction, double rotation) {

        // try to compensate for unintended rotation introduced by drivetrain bias
        double heading = 0;  //gyroscope.getAngle(); don't use gyro

        // println for debug
//        System.out.println("drive M R D " + magnitude + " " + rotation + " " + direction);
        fourMotorDrive.mecanumDrive_Polar(magnitude, direction, rotation);

        // update the Message Center if anything changed
        displayCommandedInputs(magnitude, direction, rotation);
    }

    public void operatorDrive(double driveStickXaxis, double driveStickYaxis,
                              double driveStickZaxis,
                              double magnitude, double direction,
                              boolean driverRelativeSteering) {


//        if (DriverStation.getInstance().isEnabled()) {
//            fourMotorDrive.mecanumDrive_Polar(magnitude, direction, driveStickZaxis);
//        }

//        double heading = driverRelativeSteering ? gyroscope.getAngle() : 0;
        //gyro coded out
        double heading = 0;

        if (DriverStation.getInstance().isEnabled()) {
             fourMotorDrive.mecanumDrive_Cartesian(-driveStickXaxis, -driveStickYaxis,
                                                   driveStickZaxis, heading);
             // println for debug
//             System.out.println("operatorDrive X Y Z " + driveStickXaxis + " " +
//                                                         driveStickYaxis + " " + driveStickZaxis);
        }
    }

//    public double getFrontLeftDistance() {
//
//        return Math.abs(frontLeftDriveEncoder.getDistance());
//    }
//
//    public double getFrontRightDistance() {
//
//        return Math.abs(frontRightDriveEncoder.getDistance());
//    }
//
//    public double getRearLeftDistance() {
//
//        return Math.abs(rearLeftDriveEncoder.getDistance());
//    }
//
//    public double getRearRightDistance() {
//
//        return Math.abs(rearRightDriveEncoder.getDistance());
//    }
}